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SUMMARY OF THE REPORT 


This report presents preliminary results obtained from a research 
grant entitled "Capaciflector-based Control and Imaging," with Grant 
Number NAG 5-780, for the period between February 1, 1993 and August 1, 
1993. 


This report deals with the development of a robotic system which is 
used to evaluate the feasibility of servicing and repairing the Hubble Space 
Telescope (HST) using robots. In particular, the task of opening the HST’s 
toolbox is considered in this report. First the main components of the 
robotic system will be introduced. Then each component will be described 
in detail from low-level to high-level. Finally tasks that have been 
accomplished during the reporting period for the development of the robotic 
system will be presented. Listings of source codes for the accomplished 
tasks are given at the end of the report. 


l 



1. THE ROBOTIC SYSTEM 


The task of opening the Hubble Space Telescope's Tool Box using the RRC 
K2 series robotic arm requires the use of a complex system made of computers, 
sensors, and controllers. Figure 1 shows a block diagram of the system currently 
in use to achieve this task. 



Internet 

High Level ^ ^ Low Level 

Figure 1 . Block diagram of robotics system. 

As can be seen in Figure 1, the software development is done on both 
UNIX and MS-DOS platforms. The software in both platforms is written in 
C/C++ and the communication between systems is accomplished with the use of 
the Simple Sockets Library (SSL) [1], The UNIX software can run on any UNIX 
machine connected to the Goddard Network. The MS-DOS software runs on a 
80386 machine equipped with two RS-485 ports and an OMEGA analog-to-digital 
(A/D) and digital-to-analog (D/A) board. Accomplished tasks for the development 
of the above system are presented in the following sections. 


2. CAPACIFLECTOR 

The capaciflector [2,3] is a capacitive sensor which has an increased 
sensitivity due to a shield that is placed between the sensor and ground. This 
shield prevents field lines from the sensor to go directly to ground. To accomplish 
this, a voltage follower is connected from the sensor to the shield, thereby keeping 
the shield at the same potential as the sensor. The capaciflector sensors and 
electronics that were in use were very susceptible to electrical noise emitted by the 
robot. The typical sensor setup is shown in Figure 2a. As seen in this figure, the 
capaciflector's shield is mounted over the robot ground. This setup works well as 
long as the robot is not powered. But when it is powered, the noise generated by 
the robot is induced into the shield which keeps it from being at exactly the same 
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potential as the sensor. This problem is solved by separating the sensor's circuit 
ground from the robot ground and "shielding" the shield with the sensor ground 
(see Figure 2b). The above separation resulted in a signal with substantially less 
noise than previously obtained. 
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Figure 2a. Previous sensor setup. 


Figure 2b. Improved sensor setup. 


3. SENSOR SOFTWARE (MS-DOS) 

In order to reduce the amount of space and cables required to use eight 
capaciflector sensors, a circuit that multiplexes eight sensors into two analog to 
digital converters was developed. This circuit requires that the multiplex control 
signals be generated externally. This is done by a 80386 computer with an 
OMEGA board that has digital output and analog to digital (A/D) input channels. 
Figure 3 shows a block diagram of the sensor interface to the 80386. It was 



80386 Computer Capaciflector circuit 


Figure 3. Block diagram of the sensor connection to a 80386 computer. 
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found through experimentation that the sensor circuit does not respond reliably to 
a fast multiplexing control signal due to the settling time of an integrated circuit 
not being obeyed. Consequently a complex method using a clock generated 
computer interrupt was developed to read all eight sensors at the fastest reliable 
rate. This algorithm schedules the set-up, A/D conversion, and the read operations 
for each sensor in the following manner: 
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Where sumax = set-up multiplexer A, sensor x; sumbx = set-up multiplexer B, 
sensor x; adcax = start the A/D conversion on multiplexer a, sensor x; adcbx = 
start the A/D conversion on multiplexer b, sensor x; rdax = read A/D value from 
multiplexer a, sensor x; rdbx = read A/D value from multiplexer b, sensor x. 

The fastest clock fequency that the sensor circuit can work reliably is 20Hz. 
Therefore with the interrupt scheduled algorithm there is a 50mS settling time 
between the set-up and start of the A/D conversion and 50mS between the start of 
conversion and 50mS between the start of conversion and the read of the A/D 
value. At this rate each sensor is read at 2.5 Hz. This slow rate limits the velocity 
at which the robot can move since at a fast velocity the robot can hit an object 
before it is detected by the sensor. To take care of this problem a second mode 
(static mode) of operation was added to the software. In this new mode one sensor 
is selected from multiplexer A and one from multiplexer B. The two sensors are 
then read continuously. Since no multiplexing is involved, there is no setling time 
that has to be obeyed and each sensor can be read as fast as the 80386 computer 
and the OMEGA board can read them. The current rate for the static mode is 
25Hz per sensor. At this new rate the robot can operate at faster speeds. 

After the analog signals from the sensors have been converted to digital 
values they go through several processes. Since each sensor outputs a different 
voltage when it is far from an object, the sensors have to be normalized so that 
each sensor reads zero at that distance. Then they are scaled so that each sensor 
reads 10 when it is touching an object. The equation used to normalize and scale 
is the following: 


V=10 


S n Si n 

10-SL 


( 1 ) 


where S n is the value from sensor n and Si n is the intial value (far from any 
object) from sensor n. It has been found through experimentation that any two 
sensors which have the same surface area will exibit the same responce after their 
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signals have been normalized and scaled using Equation 1 . If one sensor is bigger 
than the other, the bigger sensor will always exhibit a higher reading than the 
smaller sensor. However the two readings will converge to 10 as the sensors get 
close to an object, therefore even if the sensors have a different size or shape their 
values can be used after being processed by equation 1 for such tasks as leveling 
or centering as long as the task is performed close enough to the object. 


FREQUENCY RESPONSE 



(a) 

FREQUENCY RESPONSE 



(b) 

Figure 4. Frequency response of (a) run-length-average filter and (b) Chebyshev filter. 
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After the sensor readings have been normalized and scaled they are filtered 
using two algorithms: a run-length-average of the last 5 samples, or a chebyshev 
fourth order filter. As seen from Figure 4, the chebyshev filter has the best 
frequency response. However since the nyquist criterion is not obeyed in the 
sampling process (there is no low pass filter in front of the A/D converter) the run- 
length-average filter, which is less sensitive to aliasing, is being used until a low 
pass filter is installed. The sensor control software is shown in Appendix 1 and 
the graphical display routines are shown in Appendix 2. 

4. SENSOR SOFTWARE (UNIX) 

In order to interface the MS-DOS computer controlling the sensor hardware 
to the path planning software running on UNIX, a server using the Simple Sockets 
Library (SSL) was developed. This server can read the sensors on the MS-DOS 
PC and send them asynchronously to up to five different clients running in any 
machine connected to the Internet network. One of the clients must be the path 
planning software and the other four can be graphical display programs. The 
software is shown in Appendix 3. 

5. GRIPPER SOFTWARE (MS-DOS & UNIX) 

The gripper software is an interface between the path planning program and 
the gripper controller. The UNIX portion of the software is a set of subroutines 
that when called, send the type of action required from the gripper and a parameter 
specifying the amount of that action to the MS-DOS computer through the Internet 
network. The MS-DOS computer in turn sends those commands to the gripper 
controller through a RS-485 port. 

Since the gripper controller can not receive a new command until the 
previous one has been completed, the MS-DOS software has to monitor the 
gripper while also controlling the sensor electronics and performing the signal 
processing described above. After a gripper command has been completed, a 
simple handshake signals the UNIX portion of the software. Appendix 4 shows 
both the UNIX and MS-DOS portions of the software. 

6. PATH PLANNING SOFTWARE 

The path planning software is a program that sequentially performs robot 
tasks. Each task is not performed until the previous one has been completed to a 
certain degree of success. Opening of the tool box requires three basic operations: 
pulling a pip-pin, unlocking a latch mechanism, and opening the tool-box door. 
Since the robot’s end effector starts at an arbitrary position, there are three 
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rotational and three linear displacement unknowns at each point of interest in the 
tool-box. By leveling the end-effector against the surface of the tool-box, two of 
the three rotational unknowns are removed. By leveling against the handlebar of 
the tool-box, the third rotational unknown is removed. 

The leveling task is performed by reading two sensors, si and s2, that are 
located at the extremities of the fingers and rotating the gripper in order to 
minimize the error equation e = Isl - s2l. 

The algorithm that performs the leveling takes into account the size and 
distance between the sensors used to level. It also accounts for numerical errors 
produced in the calculation of the inverse-Jacobian which in turn cause translation 
errors of the end effector. The basic equation for leveling is given by 

(S1K1 -S2K2) 

ROT = Kr 

max(Sl,S2) (2) 

where ROT is the rotation to be performed in radians, Kq and K 2 are 
proportionality constants used when the sensors are not of the same size, and K r is 
a conversion factor which takes into account the distance between the fingers. The 
term in the denominator attenuates the rotation when the fingers get close the 
leveling surface. The displacement of the end effector while leveling isexpressed 
as 


d = [V L - max(Sl,S2)]K d -ROT- Kj 

where Vl is the voltage at which the leveling is performed, K<j is the displacement 
gain, and the term ROT Kj compensates the displacement error introduced by the 
inverse-Jacobian when a rotation is performed. 

7. CONCLUSION 

In this report we have considered the development of a robotic system used 
to evaluate the feasibility of servicing and repairing the Hubble Space Telescope 
(HST) using robots. All the components of the system were described from low- 
level to high-level and recommendations were made on how to increase the 
sensitivity and performance of the Capaciflector. The task of opening the HST's 
toolbox was presented with an emphasis on how the three rotational unknowns are 
removed. The source code for all the accomplished tasks was presented in the 
appendices. Future research can be directed to studying new control algorithms 
such as a six degree of freedom virtual force control to implement a class of 
impedance control using Capaciflectors. 


7 



8. REFERENCES 


[1] Campbell, Jr., Charles E., "The Simple Socket Library," Goddard Space 
Flight Center, Internal Distribution, October, 1992. 

[2] Vranish, J.M., R.L. McConnell, and Mahalingam, S., "Capaciflector 
Collision Avoidance Sensors for Robots," Computers and Electrical 
Engineering: An International Journal, Vol. 17, Number 3, pp. 173-180, 
1991. 

[3] Nguyen, C.C., "Capaciflector-Based Virtual Force Control and 
Centering," Semianual Report, Grant NAG 5-780, The Catholic 
University of America, Goddard Space Flight Center, February, 1993. 


8 



Appendix 1 . Sensor control software. 


/* 

* FACILITY: 

* NASA Goddard Robotics Lab 

* Engineering Test Bed 

* 

* ABSTRACT: 

* Subroutines to read the new sensors. 

* 

* DESIGN DECISIONS: 

* In order for the new sensors to work properly, an interrupt handler 

* is used to multiplex and read the sensor values at a low rate, instead 

* of the fast rate used for the old sensors (in the microseconds range). 

* 

* MODIFICATION HISTORY: 

* 15 April 1993 Paulo Uribe, CUA 

* Created 

* 24 June 1993 Paulo Uribe, CUA 

* Added a 4th order Chebyshev filter with cutoff frequency at .2pi 

V 

#include "newsens.h" 

/* filter variables */ 

int cap_average[9][A VERAGE_N], cap_pntr[9], dc_offset[9]; 

double cap_x[9][AVERAGE_N]; 

double cap_y [9] [ A VER AGE_N] ; 

double cap_z [9] [A VER AGE_N] ; 

double hp[2]; 

double a[]= {0.0, 1.4996, -0.8482); 
double aa[]={ 0.0, 1.5548,-0.6493); 
double b[]={.001836, 2.0, 1.0); 
double gain[9); 

extern int raw_skin_data[20]; 

/* Interrupt routine variables */ 
int interrupt_enable=0, index, sens_num, cap_val[9]; 
int read_s[18]=( 0,4,1,5,2,63,7,8,0,4,1,5,2,6,3,7,8 ) ; 
int set_up[18]=( 1,5,2,63,7,8,0,4,1,5,2,6,3,7,8,0,4 ) ; 
int a_to_d[18]={ 1,0, 1,0, 1,0, 1,2, 0,1, 0,1, 0,1, 0,1, 2, 0 } ; 

/* Sensor number to mux address conversion array */ 

char dbit[9]={ 1+16,2+16,4+16,8+16, 1+32,2+32,4+32, 8+32,0 ); 

int muxl_channel = MUX1_CHANNEL; 
int mux2_channel = MUX2_CHANNEL; 

void mux(int enable) 

/* Enables/disables multiplexing of the sensors, 
input paramter: 

enable = 1, enables muliplexing 
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= 0, disables multiplexing 


Note: When multiplexing is disabled the sensors set by the set_mux_channel() 
function are read continuously. 

*/ 

( 

if(enable) interrupt_enabie = 1; 
else 
( 

interrupt_enable = 0; 

outp(AD_ADDRESS+9 , 0); /* Reset the Omega board */ 

} 

} 

int set_mux_channel(unsigned int muxl, unsigned int mux2) 

/* Sets the channels to read from each mux when the function mux(0) 
has been called, 
input parameters: 

muxl = integer from 0 to 3 
mux2 = integer from 4 to 7 


*/ 

{ 


} 


ouput parameters: 

0 = mux 1 or mux2 out of range, values not set 

1 = values set 


if(muxl < 4 && (mux2 > 3 && mux2 < 8) ) 

( 

muxl_channel = muxl; 
mux2_channel = mux2; 
ADC3oard_In(mux 1 ,0,0); 
ADCJ3oard_In(mux2,0,0); 
ADC_Board _In(mux 1 ,0,0); 
ADCJBoard_In(mux2,0,0); 
ADC_Board_In(mux 1 ,0,0); 
ADC_Board_In(mux2,0,0); 
return 1; /* ok */ 

} 

else 

{ 

return 0; /* error */ 

} 


int ADC_Board_In( int sensor_number, int normalize, int filter) 

/* Read Analog Value from the Boards 
global paramaters: 

interrupt_enable = 0 reads the channels defined by MUX1_CHANNEL and 

MUX2_CHANNEL and returns 0 for any other sensor number. 
= 1 returns the sensor values read by the interrupt handler. 

input parameters: 

sensor_number = sensor to read ( integer from 0 to 7 ) 
normalize = integer 0 for no normalization of the sensor 
= integer 1 to normalize the sensor 
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filter = integer 0 for no filtering of the sensor data 

= integer 1 to filter the sensor data return value: 
integer from -2048 to 2048 representing the sensor value from -10V to 10V. 

*/ 

{ 

register i; 

int value, omega_channel,n; 
long sum; 

double dsuml,dsum2; 

if(sensor_number==8) 

{ 

/* Limit cycling detection */ 

dsuml=(double)(raw_skin_data[0]-2048)-0.617*hp[l]+l.l 1 l*hp[0]; 

value=abs((int)(0.617*dsuml-1.234*hp[0]+0.617*hp[l])); 

hp[l]=hp[0]; 

hp[0]=dsuml; 

return value; 

} 

else 

{ 

if ( interrupt_enable ) 

( 

value = cap_val [sensor_number]; 

} 

else if (sensor_number==muxl_channel II sensor_number==mux2_channel ) 

{ 

if(sensor_number < 8) 

( 

omega_channel=l; 

if(sensor_number < 4) omega_channel=0; 
outp(AD__ADDRESS + OxOB, dbit[sensor_number] & OxOF ); 
outp(AD_ADDRESS + OxOB, dbit[sensor_n umber] ); /* Select Analog 

input */ 

outp(AD_ADDRESS + OxOB, dbit[sensor_number] & OxOF ); 

} 

outp(AD_ADDRESS+l , omega_channel ); /* Select Omega Board input 

channel */ 

outp(AD_ADDRESS+2 , 0); /* Start conversion */ 

while( (inp(AD_ADDRESS) & OxCO) !=0x40); /* Wait for conversion to end 


value = inpw(AD_ADDRESS+3); 

} 

else return 0; /* return 0 if sensor_number is out of range */ 

/* remove any dc offsets and normalize the gains */ 
if(normalize) 

( 

value -= dc_offset[sensor_number]; 

value = (int) ((double)value * gain[sensor_number]); 
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if(value>2048) value=2048; 
else if(value<-2048) value=-2048; 

) 

} 

/* average last AVERAGE_N points */ 
if(filter==l) 

( 

cap_pntr[sensor_number]++; 

if(cap_pntr[sensor_number]>AVERAGE_N-l) cap_pntr[sensor_number]=0; 
sum=0; 

cap_average[sensor_number][cap_pntr[sensor_number]]=value; 

for(i=0;i<AVERAGE_N;i++) 

( 

sum+=(long)cap_average[sensor_number][i]; 

) 

value=(int)sum/AVERAGE_N; 

} 

else if(filter==2) 

{ 

n=sensor_number; 
for(i=2;i;i— ) 

{ 

cap_x[n][i]=cap_x[n][i-l]; 
cap_y[n][i]=cap_y[n] [i- 1 ]; 

} 

. cap_x[n][0]=(double)value+a[l]*cap_x[n][l]+a[2]*cap_x[n][2]; 

dsuml=cap_x[n][0]+2*cap_x[n][l]+cap_x[n][2]; 
cap_y [n] [0]=dsum 1 +aa[ 1 ] *cap_y [n] [ 1 ]+aa[2] *cap_y [n] [2] ; 
dsum2=cap_y [n] [0]+2*cap_y[n] [ 1 ]+cap_y[n][2]; 
value=(int)(dsum2*b[0]); 

} 

else if(filter==3) 

( 

n=sensor_number; 
for(i=2;i;i — ) 

( 

cap_x [n] [i]=cap_x [n] [i- 1 ] ; 
cap_y[n][i]=cap_y[n][i-l]; 
cap_z[n][i]=cap_z[n][i-l]; 

) 

/* First stage (2nd order filter) */ 

cap_x[n][0]=(double)value+1.2686*cap_x[n][l]-0.7051*cap_x(n][2]; 

dsuml=cap_x[n][0]+2*cap_x[n][l]+cap_x[n][2]; 

/* Second stage (2nd order filter) */ 

cap_y[n][0]=dsuml+1.0106*cap_y[n][l]-0.3583*cap_y[n][2]; 

dsuml=cap_y[n][0]+2*cap_y[n][l]+cap_y[n][2]; 

/* Third stage (2nd order filter) */ 

cap_z[n][0]=dsum 1+0.9044 *cap_z[n][l]-0. 2 155*cap_z[n][2]; 
dsum l=cap_z[n] [0]+2*cap_z[n] [ 1 ]+cap_z[n][2]; 

value=(int)(dsum 1 *0.0007378); 
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) 

if(sensor_number==8) 

{ 

/* Limit cycling detection */ 

if(value>3072) printf("\n*** TOUCHING PIPPIN HANDLE ***"); 
else if(value>21 19) printf("\n*** LIMIT CYCLE ***"); 

} 


return value; 


void interrupt far sensor JnterruptJiandler(void) 

/* This interrupt handler reads the sensors following the pattern described 

by the read_s[], set_up[], and a_to_d[] arrays. The index to these arrays 
is incremented in each interrupt cycle. When it reaches MAX_INDEX the 
index is reset to 0. During cycle X the sensor to be read is held in 
read_s[X], the omega board channel of the sensor that will be read in the 
next cycle is held in a_to_d[X], and the number of the sensor that will 
be read to cycles ahead is held in set_up[X]. This procedure mimizes the 
settling time required by the rms-dc converters in the sensor electronics. 

In order to increase the sampling rate of the sensors, a different word has 
to be loaded into the timer/counter chip’s divide register of the PC. This 
is done using the set_timer() function. 

*/ 

( 

if(interrupt_enable) 

( 

index++; 

if( index > MAXJNDEX ) index=0; 

cap_val[ read_s[index] ] = inpw(AD_ADDRESS+3); 

sens_num = set_up[index]; 

if(sens_num < 8) 

{ 

outp(AD_ADDRESS + OxOB, dbit[sens_num] & OxOF ); 
outp(AD_ADDRESS + OxOB, dbit[sens_num] ); /* Select Analog input */ 
outp(AD_ ADDRESS + OxOB, dbit[sens_num] ); /* Select Analog input */ 
outp(AD_ADDRESS + OxOB, dbit[sens_num] & OxOF ); 

} 

outp(AD_ADDRESS+l t a_to_d [index] ); 
outp(AD_ADDRESS+2 , 0); 

) /* End Interrupt Enable */ 

/* hand control to other timer interrupt service routines */ 

(*sensor_interrupt_o!dhandler)(); 

} 
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void initialize_sensors(void) 

/* Removes any dc offsets and sets the gains of each sensor in order for them 
to have the same response. 

*/ 

{ 

unsigned int i,j,k; 

int cap_value_a[9],cap_value_b[9],cap,omega_mux, first; 
long sum; 
char string[30]; 

interrupt_enable = 0; 

outp(AD_ADDRESS+9 , 0); /* Reset the Omega board */ 

for(i=0;i<SENSNUM;i++) 

{ 

_settextcolor(10); 

_settextposition(4,l); 

sprintf(string, "Initializing offsets and gains for sensor: %d’\i); 
_outtext(string); 

if(i<8) 

{ 

outp(AD_ ADDRESS + OxOB, dbit[i] & OxOF ); 

outp( AD_ADDRES S + OxOB, dbit[i] ); /* Select Analog input */ 

outp(AD_ADDRESS + OxOB, dbit[i] & OxOF ); 

} 

omega_mux=2; 
if(i<8) omega_mux=l; 
if (i<4) omega_mux=0; 

for(j=0;j<60000;j++); 

sum=0; 

first=0; 

for(j=0;j<101;j++) 

{ 

outp(AD_ADDRESS+l , omega_mux ); 
outp(AD_ADDRESS+2 , 0); 

while( (inp(AD_ADDRESS) & OxCO) !=0x40); 

cap = inpw(AD_ADDRESS+3); 

if( [first ) first=l; /* Throw out first value */ 

else sum += (long)cap; 

for(k=0;k<1000;k++); 

} 

sum /= 100; 

cap_value_a[i] = (int)sum; 
cap_value_b[i] = 2048; 
dc_offset[i] = cap_value_a[i]; 
if(cap_value_b[i] - dc_offset[i]) 
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gain[i] = 2048.0 / (double)(cap_value_b[i] - dc_offset[i]); 
else gain[i]= 10; /* Error, set gain to 10 */ 
if(gain[i] > 1.50) 

( 

_settextcolor(12); 

_settextposition(5,l); 

sprintf(string, "WARNING; Gain greater than 1.5 for sensor(s) ”); 
_outtext(string); 

_settextposition(5,44+i*2); 
sprintf(string,"%d ",i); 
if(gain[i]==10) _settextcolor(14); 

_outtext(string); 

) 


} 

) 

void set_timer(char high_byte, char low_byte) 

/* Sets the divide register of the timer/counter chip in the PC. 

The value loaded by the BIOS is 0xD86E. This value represents 
an 18.2Hz interrupt. A lower value speeds up the timer. 

*/ 

{ 

_disable() ; 
outp(0x43,0x34) ; 

outp(0x40,Iow _byte) ; /* low byte */ 

outp(0x40,high_byte) ; /* high byte */ 

/*make sure pic can handle interrupt*/ 
outp(EOIADDR,0x20); 

_enable(); 

} 

void install_sensorJnterrupt(void) 

/* installs the new sensor interrupt */ 

( 

/* Get existing timer vector */ 

_disable(); 

sensor Jnterrupt_oldhandler = _dos_getvect( Oxlc ); 

_enable(); 

_disable(); 

_dos_setvect( Oxlc, sensorjnterruptjiandler ); 

_enable(); 

} 

void uninstall_sensor_interrupt(void) 

/* restores the timer interupt to the old vector */ 

{ 

_disable(); 

_dos_setvect(0xlc, sensor_interrupt_oIdhandler); 

_enable(); 

i 

void print_gain(void) 

/* prints the dc offsets and gains for each sensor */ 

{ 
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char string [60]; 
int i; 

_settextcolor(12); 

_settextposition(6,10); 

_settextcolor(15); 

for(i=0;i<9;i++) 

{ 

_settextposition(7+i, 1 0); 

sprintf(string, M Sensor #%2d: DC offset: %4d Gain:%3.3f , t i4c^offsct[i],gain[i]); 
_outtext(string); 

) 

_settextpos ition( 1 6 ,1 0) ; 

_settextcolor(12); 

_outtext("===== Press any key to continue ========="); 

getch(); 


Appendix 2. Graphical display routines. 


void display(int graphon,int norm, int filter, int raw_skin_data[]) 
/* input parameters: graphon = 0 Display text only 

= 1 Display graphics 

norm = 0 normalizing and scaling is off 
= 1 normalizing and scaling is on 

filter = 0 No filtering 

= 1 Run length average filter 
= 2 Chevyshev filter 
= 3 Butterworth filter 
raw_skin_data[] data to be plotted 

*/ 

{ 

register ij; 

char string [60]; 

int temp; 

double tempf; 

static char buf[BUFSIZE]; 

_settextcolor(15); 

_settextposition( 1,1); 

_outtext("Normalizing: O"); 
if(aver) 

{ 

_settextposition( 1,15); 
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_outtext("N "); 

} 

else 

( 

_settextposition(l , 15); 

_outtext( ,, FF H ); 

} 

_settextposition(2,l); 

_outtext("Filtering : ”); 
if(lfilter) 

{ 

_settex tposition (2,14); 

_outtext("OFF "); 

} 

else if(filter==l) 

{ 

_settextposition(2, 14); 

_outtext(”FIR ”); /* Finite Impulse Response Filter */ 

} 

else if(filter==2) 

{ 

_settextposition(2,14); 

_outtext("cIIR"); /* Chebyshev HR filter */ 

) 

else if(filter==3) 

( 

_settextposition(2,14); 

j^uttextObllR”); /* Buttenvorth HR filter */ 

} 

_settextposition(l,65); 

_j)uttext("Cap-socket: ON "); 
if(rrc_socket) 

{ 

_settextcolor(15); 

_settextposition(2,65); 

_outtext("RRC-socket: ON "); 

) 

else 

( 

_settextcolor(15); 

_$ettextposition(2,65); 

_outtext( M RRC-socket; OFF"); 

} 


for(i=l;i<10;i++) 

{ 

_settextposition(l,16+i*5); 

_settextcolor(16-i); 

sprintfCstring/^d’J); 

_outtext(string); 

) 

_settextcolor(15); 

_se ttextposition (2 ,19); 

sprintf(string,"%4d %4d %4d %4d %4d %4d %4d %4d %4d n , 


17 



raw_skin_data[0],raw_skin_data[l],raw_skin_data[2], 
raw_skin_data[3] > raw_skin_data[4],raw_skin_data[5], 
raw_skin_data[6] ,raw_skin_data[7] ,data); 

/* Display numerically the values of the sensors */ 

_outtext(string); 

/* Display graphically the values of the sensors */ 
if(graphon) 

{ 

_setcolor(0); 

_moveto(xpos,80); 

_lineto(xpos,480); 

for(j=0;j<SENSEN;j++) 

t 

if( raw_skin_data[j] > 2000) 

( 

_settextcolor(15-j); 

_seuex tpos ition( 1 ,2 1 +j *5); 
sprintf(string,"%d"j+l); 

_outtext(string); 

_setcolor(15-j); 

if(sensor_num==j) 

( 

tempf = (double)((raw_skin_data[j])-2024)*constant; 
tempf+= 24; 

} 

else tempf = (double)(raw_skin_data[j]-2000); 
tempf = 170.0*logl0(fabs(temp0+l); 
temp = 680 - (int)tempf; 

_setpixel(xpos > temp); 

) 

else 

{ 

_settextcolor(7); 

_settex tposition( 1 ,2 1 +j *5); 
sprintf(string, ,, %d H j+l); 

_outtext(string); 

} 

} 

if(++xpos>600) xpos=15; 

if(xpos==15 II xpos==315) timel=time(&times); 

else if(xpos==265 II xpos==565) 

{ 

Ume2=time(&times); 

sprintf(buf, "Sampling Frequency=%2.1fHz’’,250.0/difftime(time2,timel)); 
_settextposition(5,l); 

_settextcolor(15); 

_outtext(buO; 

) 

} 

__settextposition(4,l); 

printf( M \n ,, ); 

} 
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Appendix 3. Sensor server (UNIX). 


/* 

* FACILITY: 

* NASA Goddard Robotics Lab 

* Engineering Test Bed 

* 

* ABSTRACT: 

* Capaciflector sensor server. Accepts 1 sensor client, 1 control (path planning software) 

* client and up to 4 sensor-read clients. Software written using client-server architecture. 

* 

* MODIFICATION HISTORY: 

* 15 May 1993 Paulo Uribe, CUA 

* Created 

* 20 July 1993 Paulo Uribe, CUA 

* Added capabilities for up to 4 sensor-read clients to be accepted by server. 


#include <unistd.h> 

#include <stdio.h> 

#include <math.h> 

#include <sys/times.h> 
extern "C" ( 

#include "xmath.h" 

#include "sockets. h" 

#include "xtdio.h” 

) 

#define MAXJ3UF 128 
#define DATABAD 30 
#define MAXSRCLIENTS 2 

Socket *server,*client; // server socket pctraj 
Socket ^control; // accept pc connection 
Socket *sensor; // client to sensor program 
Socket *sread[MAXSRCLIENTS]; 

void Open_server(); // open server pctraj 

void Accept_clientO; // accept pc connection 

void Connect_sensor(); // open client to sensor program 

void Close_sensorO; // close sensor connection 

void get_control_cmd(); 

void get_sr_cmd(int); 

void get_cap_valO; 

int yn(); 

char buf[MAX_BUF],cmd[MAX_BUF]; 


char ServName[]= M Capaciflector"; 

intcap_value[10]; 

float fcap[10]; 
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int run= 1 ,control_socket,sensor_socket; 

int muxflag=0,capflag=0 > sendflag=0,toutcount=0,sensortout=1000; 
int mux 1 ,mux2,mux; 
int sensnum,const_flag; 
double con; 

int shift t sr_socket T srsfIag[MAXSRCLIENTS]; 
int ss _hs=0; 
char s_hs[50]; 

void mainO 

{ 

register i; 

int flag=l,databad=DATABAD; 

fprintf(stderr/VCapaciflector server. V1.2W); 
fprintf(stderr 1 ”\nOpening server...’ 1 ); 

Open_server(); 

fprintf(stderr, "Server open\n"); 

Smaskset(server); 

Smasktime(0,10); 

while(run) 

( 

i=0; 

shift=0; 

while(i<MAXSRCLIENTS) 

( 

if(!sread[i]) 

( 

Accept_client(); 

i=MAXSRCLIENTS; 

} 

else 

{ 

sr_socket=l; 

if(Stest(sread[i])<0) ' * . 

{ 

fprintf(stderr,”\nError in sensor-read socket,"); 
sr_socket=0; 

} 

else if(Stest(sread[i])) get_sr_cmd(i); 
if(!sr_socket) 

{ 

fprintf(stderr,"\nClosing sensor-read clientNn"); 
Sclose(sread[i]); 
sread[i]=NULL; 
shift = 1; 


} 

i++; 

)//End MAXCLIENTS 
i=0; 

while(shift) 

{ 
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shift=0; 

while(kMAXSRCLIENTS) 

{ 

if(!sread[i] && i<MAXSRCLIENTS-l) 

( 

if(sread[i+l]) shift=l; 

sread[i]=sread[i+l]; 

sread[i+l]=NULL; 

} 

i++; 

} 

}// End While(Shift) 
if(control==NULL) Accept_dient(); 
else 
( 

if(ss_hs&& ! databad) 

{ 

Sprintf(control,s_hs); 
if(sensor) Sprintf(sensor,”sk"); 
ss_hs=0; 

} 

control_socket=l; 

if(Stest(control)<0) 

{ 

fprintf(stderr,"\nError in control socket."); 
control_socket=0; 

} 

else if(Stest(control)) get_control_cmd(); 
if(!control_socket) 

{ 

fprintf(stderr,"\nClosing control client\n"); 

Sclose(control); 

control=NULL; 

} 

} 

if(capflag) 

{ 

if (! databad) 

{ 

for(i=0;i<8;i++) 

fcap[i]=10*( cap_value[i] - 2048.0 )/2048.0; 
fcap[8]=(int)cap_value[8]; 

sprintf(buf,"qc %2.4f %2.4f %2.4f %2.4f %2.4f %2.4f %2.4f %2.4f %2.4r , 
fcap[0],fcap[l], 
fcap[2],fcap[3], 
fcap[4],fcap[5], 
fcap[6],fcap[7],fcap[8]); 
if(sendflag && control) 

{ 

Sputs(buf, control); 
sendflag=0; 
capflag=0; 


else 
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{ 


1 = 0 ; 

while( sread[i] && kMAXSRCLIENTS) 

{ 

if(srsflag[i] && sread[i]) 

( 

Sputs(buf,sread[i]); 

srsflag[i]=0; 

capflag=0; 

} 

i++; 

} 

} 

)//End if(!databad) 
else 
( 

/* The first several sensor readings after a change in a mux channel are bad. Therefore when a 
change in any mux channel is detected, databad is set to DATABAD and while databad>0 
capflag is set to zero to stop any transmition of the data to any of the sensor-read and 
control clients. */ 

fprintf(stderr,’V[bad data %2d ]", databad-); 
fprintf(stderr,’V V); 

capflag=0; 

} 

}//End if(capflag) 
if(muxflag && sensor) 

{ 

databad=DATABAD; 

muxflag=0; 

sprintf(buf,"ss %d %d %d",muxl,mux2,mux); 

Sputs(buf, sensor); 

} 


if(sensor==NULL) Accept_client(); 
else 
{ 

sensor_socket=l; 

if(const_flag) 

{ 

Sprintf(sensor,”sg %d %f',sensnum,con); 
const_flag=0; 

} 

if(flag) 

( 

sensortout=500; 

flag=0; 

Sprintf(sensor,"se"); 

} 

else if(!scnsortout) 

( 

fprintf(stderr,'Y Sensor timed out. Trying again(%d)...\r",toutcount); 
toutcount++; 

if(toutcount>100) sensor_sockei=0; 
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flag=l; 

} 

if(Stesl(sensor)<0) 

{ 

fprintf(stderr/ViEiTor in sensor socket."); 
sensor_socket=0; 

} 

else if(Stest(sensor)) 

{ 

get_cap_val(); 

flag=l; 


else sensortout--; 
if(!sensor_socket) 

( 

fprintf(stderr i ”\nClosing sensor client\n"); 

Sclose(sensor); 

sensor=NULL; 


) // End else 
) // End while(run) 
fprintf(stderr, M \nExiting M ); 
i=0; 

while(sread[i] && i<MAXSRCLIENTS) 

{ 

if(sread[i]) 

{ 

fprintf(stderr,"\nClosing sensor-read client %d",i); 
Sprintf(sread[i],"sq"); 

Sclose(sread[i]); 

} 

i++; 

} 

if(control) 

{ 

fprintf(stderr r "\nClosing control client 1 '); 
Sprintf(control,"sq"); 

Sclose(control); 

} 

if(sensor) 

{ 

fprintf(stdenV\nClosing sensor client”); 
SprintfCsensor/’sq”); 

Sclose(sensor); 

} 

fprintf(stderr/ViClosing serveAn"); 
Smaskunset(server); 

Sclose(server); 

} 

void Open_server() 

( 

int tr=2; 
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while(tr) 

{ 

server = Sopen (ServName, "c"); 
if(server) 

( 

Sprintf(server,"cp"); 
if(tr==2) fprintf(stderr, 

"\nWARNING: <%s> Server already open. Will try to close it.,.”, 
ServName); 

Sprintf(server,"cQ M ); 

Sclose(server); 

sleep(l); 

tr~; 

} 

else 

{ 

break; 

} 

} 

if(!tr) 

{ 

fprintf(stderr, 

"VtCould not close it. Press [RETURN] to start a new server"); 
getchar(stderr); 

} 

else if(tr==l) 

fprintf(stderr, ,f ViPrevious server closed. Opening new server..."); 

server = Sopen (ServName, "s"); 

if (! server) 

{ 

// If the Server already exists remove it 

Srmsrvr (ServName); 

server = Sopen (ServName, "s"); 

if (! server) ' ^ 

( 

fprintf(stderr,"\nUnable to open <SensorServer> as server..\n"); 
exit(O); 

} 

) 

j 

void Accept_client() 

{ // Accept Pc Connection 
register i; 

if( Smaskwaitf) ) client = Saccept( server ); 
else 
( 

return; 

} 

if(client!=NULL) 

{ 

i=40; 

while(!Stest(c!ient) && i-) fprintf(stderr,"."); 
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/* Wait for client to identify itself */ 
if(Stest(client)) 

{ 

Sgets ( buf, MAX_BUF, client ); 
if (buf[0]=='s' && buf[l]=='n') 

( 

if (sensor==NULL) 

( 

sensor=cIient; 

fprintf(stderr, M ViPC connectedW); 

j 

else 

( 

fprintf(stderr, ,r \jiERROR: A second sensor client has been connected'’); 
fprintf(stden\"VL Closing it..."); 

Sprintf(client,"sQ"); 

Sclose(client); 
fprintf(stderr, "Closed."); 

} 

} 

else if (buf[0]== , s I && buf[l]=='r') 

{ 

i=0; 

while( sread[i] && i<MAXSRCLIENTS ) i++; 
if ( i==MAXSRCLIENTS ) 

{ 

fprintf(stderr, 

"ViERROR: A fifth sensor read client has been connected"); 
fprintf(stderr, M Vi Closing it...”); 

Sprintf(client, M sQ"); 

Sclose(client); 

fprintf(stderr,"Closed.\n”); 

} 

else 

{ 

sread[i]=client; 

fprintf(stderr,"ViSensor-read client connected\n H ); 

} 

} 

else if (buf[0]=’c' && buf[l]=='p’) 

{ 

if (control==NULL) 

{ 

control=client; 

fprintf(stderr,’ViControl program connectedW’); 

} 

else 

( 

fprintf(stderr, 

,f \nERROR: A second control program client has been connected”); 
fprintf(stderr,"\n Closing it..."); 

SprintfCclientf'sQ"); 

Sclose(client); 

fprinif(stden\ ,, CIosed.W‘); 
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} 

) 

else 

{ 

fprintf(stderr, 

”\nERR.OR: Unknown client has been connected<%s>",buf); 
fprintf(stderr,”\n Closing it...*’); 

Sprintf(client,"sQ"); 

Sclose(client); 

fprintf(stderr/ , Closed.Nn"); 

} 

} // End if(Stest(client)) 
else 
( 

fprintf(stderr, 

’ViERROR: Unknown client has been connected”); 
fprintf(stden\”\n Closing it..."); 

Sprintf(client/'sQ"); 

Sclose(client); 

fprintf(stderr,"Closed.\n n ); 

} 

) //End if(client) 

else fprintf(stden\ ,r ViError accepting client\n”); 

fprintf(stderr,"\n\n\nClients: ,t ); 

if(sensor) fprint^stderr/ViPC sensor controller”); 

if(control) fprintf(stderr,”ViControl software”); 

for(i=0;i<MAXSRCLIENTS&&sread[i];i++); 

if(i) fprintf(stderr,”\n%d Sensor-read clients”); 

fprintf(stderr, ,r \n"); 

} 

int yn() 

{ 

char c; 

c=getc(stdin); 
if(c— y’llc=- Y’) 

( 

fprintfCstderr/'Y"); 
return 1; 

} 

fprintf(stderr,"N"); 
return 0; 

} 


void get_cap_val() 

{ 

char cmd[MAX_BLTF]; 
int i,junk,muxl,mux2,mux; 
int dcap_value(10]; 

cmd[0]=cmd[l]=' 
buf[0]=buf[ 1 ]=’ 
Sgets(buf,MAX_BUF .sensor); 
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sscanf(buf t ’'%s’\cmd); 

switch(cmd[0]) 

{ 

case ’s’: 

toutcount=0; 

switch(cmd[l]) 

{ 

case Tc': /* Read the sensor values from the PC client */ 

sscanf(buf,”%s %d %d %d %d %d %d %d %d %d %d”, 
cmd,&junk,&dcap_value[0],&dcap_value[l], 
&dcap_value[2] ,&dcap_value[3] , 
&dcap_value[4],&dcap_value[5], 
&dcap_value[6],&dcap_value[7],&dcap_value[8]); 
for(i=0;i<9;i++) cap_value[i]=dcap_value[i]; 
capflag=l; 
break; 

case ’Q': /* Shut down */ 
run=0; 

case 'q 1 : /* Close client */ 
sensor_socket=0; 
break; 


default: 

break; 

} 

break; 
case V: 

switch(cmd[l]) 

{ 

case 's': /* Mux change handshake */ 
sscanf(buf,"%s %d %d %d’’,cmd,&muxl,&mux2,&mux); 
ss_hs=l; 

sprintf(s_hs,"rs %d %d %d",muxl,mux2,rnux); 
break; 
default: 
break; 


break; 

default: 

break; 


} 

void get_control_cmd() 

{ 


cmd[0]=cmd[l]=' 

buf[0]=buf[l]=’ 

Sgets(buf,MAX_BUF,control); 

sscanf(buf,"%s",cmd); 
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switch(cmd[0]) 

{ 

case ’c f : 

switch(cmd[l]) 

{ 

case 'c': /* Clear limit cycling flag on PC */ 
if(sensor) Sprintf(sensor, M sk"); 
break; 

case 'e': /* Request sensor data */ 
sendflag=l; 
break; 

case 's’: /* Change mux channel command */ 

sscanf(buf,"%s%d%d%d M ,cmd,&muxl,&mux2,&mux); 

muxflag=l; 

break; 

case ’g’: /* Send ( to PC ) gain used in control program */ 
sscanf(buf/’%s%d%lf^cmd>&sensnum,&con); 
const_flag=l; 
break; 

case ’Q’: /* Shut down */ 
run=0; 


case 'q': /* Close client */ 
controLsocket=0; 
break; 


default: 

break; 


break; 

default: 

fprintf(stderr, u [%s] M 1 buf); 

break; 

} 

) 

void get_sr_cmd(int num) 

{ 

cmd[0]=cmd[l]=' 

buf[0]=buf[l]=' 

Sgets(buf,MAX_BUF,sread[num]); 

sscanf(buf,"%s",cmd); 

switch(cmd[0]) 

{ 

case 'c': 

switch(cmd[l]) 

( 

case ’e ? : 

srsflag[num]=l; 

break; 

case ’Q’: 
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fprintf(stderr, 

"NnSensor read client %d tryed to terminate me!",num); 
case 'q': 

sr_socket=0; 

break; 

default: 

break; 

) 

break; 

default: 

fprintf(stderr,"[%s] M ,buf); 

break; 

} 

while(Stest(sread[num])>0) Sgets(buf,MAX_BUF,sread[num]); 


/* 

* 

* 

* 

* 


Appendix 4. Gripper software. 


FACILITY: 

NASA Goddard Robotics Lab 
Engineering Test Bed 


* ABSTRACT: 

* Subroutines to send commands to the toolbox gripper. 

* 


* DESIGN DECISIONS: 

* Subroutines transmitj)yte2(), geLbyte2_ready_status(), get_byte2(), 

* and init_port_2() were copied from rrc_comm.c. 

* 


* 

* 

* 

*/ 


MODIFICATION HISTORY: 

1 June 1993 Paulo Uribe, CUA 

Created 


#include "rrc_grpr.h M 

extern Socket *rrc_socket; 
extern int gripcmd; 

int initialize _gripper(void) 

( 

int position, force, speed; 
init_port_2(); 

return !send_command( ,, MP3000",&position, &force, &speed); 

} 

void read_port(void) 

{ 

int timeout=5000; 

printfCVWaiting for gripper... Press any key if suspected crash in the little giant”); 
while(get_byte2_ready_statusO!=DATA_READY && !kbhit()); 
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if(kbhitO) getch(); 

printf('V "); 

while(get_byte2_ready_status()==DATA_READY) 

{ 

get_byte2(); 

timeout=1000; 

printf('VWaiting for next char.. .Press any key if suspected crash in the little giant”); 

while(get_byte2_ready_status()!=DATA_READY && timeout &&!kbhitO) timeout— 

if(kbhit()) getchQ; 

printfCV M ); 

} 

printf("VTimeout=%d", timeout); 

return; 

} 

int send_command(char *c,int * position, int * force, int * speed) 

{ 

register i; 

int error ,n,timeout=0; 
char r[ 15] ; 

gripcmd=0; 

for(i=0;i<6 && c[i];i++); 

if(i!=6 && c[i]) { printf( M Gripper command error <%s> ”,c); return 255; } 
for(i=0;i<6;i++) transmit_byte2(c[i]); 

Sprintf(rrc_socket, "OK”); 

printf("GRP[%dr,i); 

gripcmd=l; 

i=0; 

while(i<2 && timeout<700) 

( 

if (get_byte2_ready_statusO~DATA_READY) 

( 

r[i]=get_byte20; 

i++; 

timeout=0; 

} 

timeout++; 

} 

if(timeout==700) ( printf("Gripper time out "); return 255; ) 
n=2; 

it(r[0]=='G') n=10; 
else if(r[l]=='P') n-6; 
while(n && timeout<500) 

{ 

if (get_byte2_ready_status()==DATA_READY) 

{ 

r[i]=get_byte2(); 

i++; 

n— ; 

timeout=0; 


timeout++; 


r[i]=0; 
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if(c[0]==r[0] && c[l]==r[l]> 

{ 

switch(r[0]) 

{ 

case F: 

return r[3]; 

case ’G’: 

error=atoi(r+10); 

r[10]=0; 

*position=atoi(r+6); 

r[6]=0; 

*force=atoi(r+2); 
return error; 
case ’M’: 

switch(r[l]) 

{ 

case 'E; 

error=atoi(r+2); 
return error; 

case F: 

error=atoi(r+6); 

r[6]=0; 

*position=atoi(r+2); 
return error; 


case 'S'; 

error=atoi(r+6); 

r[6]=0; 

*speed=atoi(r+2); 
return error; 

default: 

return 1 1; 


} 

else 

{ 

return 12; 


} 


/***************************************************************** 

* 

* Procedure transmit_byte2() 

* 

* This procedure will transmit the data byte passed. 

* 

* 

int transmit_byte2(char data) 

{ 

long timeout = OL ; 
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while (!(inp(LSR_Addr_Port_2) & 0x20)) 
if (timeout++ == 100000) 

return(XMITJTIMEOUT); 

outp(TXD_Addr_Port_2, data) ; 
printf("[%c]\data); 

return(XMIT_OK) ; 

} 

y************* ********** ************** **************************** 
* 

* Procedure get_byte2_ready_status() 

* 

* This procedure will receive a data byte from the input com port 

* if one is available. If not available, the return status will 

* reflect it 

* 

* 

******************************** ********************************^ 
int get_byte2_ready_status(void) 

( 


if (!(inp(RST_Addr_Port_2) & 0x01)) 
return (NOT_READY) ; 

else 

return(DATA_READY) ; 

) 


^***************************************************************** 

* 

* Procedure get_byte2() 

* 

* This procedure will receive a data byte from the input com port 

* if one is available. If not available, the return status will 

* reflect it 

* 

* 

****************************************************************/ 
unsigned char get_byte2(void) 


retum((unsigned char) inp(RXD_Addr_Port_2)) ; 

} 


^***************** ***************************************** 

* 

* Procedure init_port_2() 

* 

* Initilize Port 1 82510 US ART to 8 bits no parity 

* 

* Divisor = 18432000/2/16/baud 

* BAL = 0x0a for 56.7 Kbaud 

* 0x3c for 9.6 Kbaud 

* OxfO for 2.4 Kbaud 

**************** *»*****************************************/ 
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void init_port_2(void) 

( 

outp(GIR_Addr_Port_2,Bank_One) ; 
outp(ICM_Addr_Port_2,0xl0) ; 
outp(GIR_Addr_Port_23ank_Zero) ; 
outp(LCR_Addr_Port_2,0x80) ; 
outp(BAH_Addr_Port_2,0) ; 

outp(B AL_Addr_Port_2,0xf0) ; /* set baud rate, see above */ 

outp(LCR_Addr_Port_2,0x07) ; 
outp(GIR_Addr_Port_2,Bank_Two) ; 
outp(TMD_Addr_Port_2,0xcl) ; 
outp(IMD_Addr_Port_2,0x0c) ; 
outp(RIE_Addr_Port_2,0x00) ; 
outp(RMD_Addr_Port_2,0x30) ; 
outp(FMD_Addr_Port_2,0x00) ; 
outp(GIR_Addr_Port_2,Bank_Three) ; 
outp(BACF_Addr_Port_2,0x04) ; 
outp(MIE_Addr_Port_2,0x00) ; 
outp(TMIE__Addr_Port_2,0x00) ; 
outp(CLCF_Addr_Port_2,0x50) ; 
outp(GIR_Addr_Port_2,Bank_One) ; 
ou tp (TC M_ Ad dr_Port_2 , Ox 02) ; 
outp(RCM_Addr_Port_2,0xb4) ; 
outp(GIR_Addr_Port_2,Bank_Zero) ; 
outp(LCR_Addr_Port_2,0x07) ; 
outp(GER_Addi_PortJ2,0x01) ; 

} 
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